# coding: utf-8

from __future__ import print_function

import can
import time


def recv():
    bus = can.interface.Bus(bustype='socketcan', channel='can0', bitrate=500000)

    try:
        msg = bus.recv(timeout=5)  # 接收信息
        if msg != None:
            print('Message received on {}'.format(bus.channel_info))
            print(msg.data)
    except can.CanError:
        print('Message receiving timeout')

def decode(msg_data):
    # TODO 数据解码，id:0x0165 ...
    acc, brake, steer, gear = msg_data[0]

    return acc, brake, steer, gear



class CarController():
    def __init__(self, *, bitrate=500000):
        self.steerRange = [-633.5, 595.3]
        self.selfDriving_req = 1
        self.stopCar_req = 0
        self.bus0 = can.interface.Bus(bustype='socketcan', channel='can0', bitrate=bitrate)
        pass

    def cansend_one(self, can_data, *, arbitration_id=0x0C01D0E1, is_extended_id=True):
        msg = can.Message(arbitration_id=arbitration_id,
                          data=can_data,
                          is_extended_id=is_extended_id)
        try:
            self.bus0.send(msg)
            print("Message sent on {}".format(self.bus0.channel_info))
        except can.CanError:
            print("Message NOT sent")

    def gear(self, gearMod: int, *, acc: int = 0, brake: int = 0, steer: float = 0.) -> None:
        '''0：空挡；1：前进；2：倒挡'''
        # if gearMod == 0:
        #     can_data = []
        # elif gearMod == 1:
        #     can_data = []
        # elif gearMod == 2:
        #     can_data = []
        # else:
        #     raise TypeError
        can_data = self.gen_candata(acc, brake, steer, gearMod)
        self.cansend_one(can_data)

    def accelerate(self, acc: int, *, brake: int = 0, steer: float = 0., gearMod: int) -> None:
        can_data = self.gen_candata(acc, brake, steer, gearMod)
        self.cansend_one(can_data)

    def brake(self, brake: int, *, acc: int = 0, steer: float = 0., gearMod: int) -> None:
        can_data = self.gen_candata(acc, brake, steer, gearMod)
        self.cansend_one(can_data)

    def steer(self, steer: float, *, acc: int = 0, brake: int = 0, gearMod: int) -> None:
        can_data = self.gen_candata(acc, brake, steer, gearMod)
        self.cansend_one(can_data)

    def gen_candata(self, acc: int, brake: int, steer: float, gearMod: int) -> list:
        if not (0 <= acc <= 70) & (0 <= brake <= 70) & (-700 <= steer <= 700):
            raise ValueError

        steer_bus_value = int((steer + 870) * 10)
        steer_low = steer_bus_value & 0x00ff
        steer_high = steer_bus_value >> 8
        candata_5th_byte = (self.stopCar_req << 3) + (gearMod << 1) + self.selfDriving_req

        # TODO: 准备发送缓冲 candata
        candata = [acc, brake, steer_high, steer_low, candata_5th_byte, 0, 0]
        return candata

    def startUp(self) -> None:

        # TODO 档位
        self.gear(0)
        self.brake(50)
        time.sleep(1)
        self.accelerate(10, brake=50, gearMod=1)
        time.sleep(0.5)
        self.brake(50)


if __name__ == '__main__':
    tbox = CarController()
    tbox.startUp()
